//
// Created by 16933 on 2022/5/2.
//

#include "Debug.h"
#include "cmsis_os.h"
#include "bsp_vofa.h"
#include "tim.h"
#include "bsp_Motor.h"
#include "inv_mpu.h"
#include "bsp_MPU6050.h"
#include "Pid.h"
uint8_t MpuResetFlag = 0;               //mpu重置信号
uint8_t MpuResetCompleteFlag = 1;       //mpu重置完成信号
extern float PID_Speed_Lout;
extern float PID_Speed_Rout;
extern float PID_Angle_Out;
extern pid_t PID_Motor_Left_Speed;    //左边电机速度环pid控制器
extern pid_t PID_Motor_Right_Speed;   //右边电机速度环pid控制器
extern pid_t PID_Motor_Angle;    //左边电机角度环pid控制器

extern float pitch;
extern float yaw;
extern float roll;
extern float offset;
extern float pitch_sum;
void DebugTask(void const * argument)
{
    HAL_TIM_Encoder_Start(&htim2,TIM_CHANNEL_ALL);
    HAL_TIM_Encoder_Start(&htim4,TIM_CHANNEL_ALL);
    uint32_t debug_time = osKernelSysTick();  //20ms为一个周期
    for(;;)
    {
//        HAL_GPIO_WritePin(TEST_GPIO_Port,TEST_Pin,GPIO_PIN_SET);
        if (MpuResetFlag)
        {
            HAL_GPIO_WritePin(GPIOC,GPIO_PIN_13,GPIO_PIN_SET);
            PID_Speed_Lout = 0;
            PID_Speed_Rout = 0;
            while (MPU_Init())
            {
                ;
            }
            while (mpu_dmp_init())
            {
                ;
            }
            for (int i = 0; i < 20; i++) {
                HAL_GPIO_WritePin(GPIOC,GPIO_PIN_13,GPIO_PIN_RESET);
                HAL_Delay(500);
                HAL_GPIO_WritePin(GPIOC,GPIO_PIN_13,GPIO_PIN_SET);
                HAL_Delay(500);
            }
            pitch_sum = 0;
            for (int i = 0; i < 30; i++) {
                HAL_GPIO_WritePin(GPIOC,GPIO_PIN_13,GPIO_PIN_RESET);
                for (int j = 0; j < 20; j++) {
                    mpu_dmp_get_data(&pitch,&roll,&yaw);
                    vTaskDelay(1);
                    pitch_sum+=pitch;
                    HAL_GPIO_WritePin(GPIOC,GPIO_PIN_13,GPIO_PIN_SET);
                }

            }
            offset = pitch_sum/600.0;
            HAL_GPIO_WritePin(GPIOC,GPIO_PIN_13,GPIO_PIN_RESET);
            MpuResetCompleteFlag = 1;
            MpuResetFlag = 0;
            HAL_GPIO_WritePin(GPIOC,GPIO_PIN_13,GPIO_PIN_RESET);
        }
//        HAL_GPIO_WritePin(TEST_GPIO_Port,TEST_Pin,GPIO_PIN_RESET);
        osDelayUntil(&debug_time,20);
    }
}